// Copyright (c) 2025, ros2_control development team
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

/*
 * Authors: Julia Jia
 */

#ifndef FORCE_TORQUE_SENSOR_BROADCASTER__WRENCH_TRANSFORMER_HPP_
#define FORCE_TORQUE_SENSOR_BROADCASTER__WRENCH_TRANSFORMER_HPP_

#include <memory>
#include <string>
#include <unordered_map>
#include <vector>

#include "geometry_msgs/msg/wrench_stamped.hpp"
#include "rclcpp/rclcpp.hpp"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include "tf2_ros/buffer.hpp"
#include "tf2_ros/transform_listener.hpp"

// auto-generated by generate_parameter_library
#include "force_torque_sensor_broadcaster/wrench_transformer_parameters.hpp"

namespace force_torque_sensor_broadcaster
{

class WrenchTransformer : public rclcpp::Node
{
public:
  explicit WrenchTransformer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());

  void init();

  ~WrenchTransformer() = default;

private:
  void wrench_callback(const geometry_msgs::msg::WrenchStamped::SharedPtr msg);
  bool transform_wrench(
    const geometry_msgs::msg::WrenchStamped & input_wrench, const std::string & target_frame,
    geometry_msgs::msg::WrenchStamped & output_wrench);

  void setup_subscriber();
  void setup_publishers();

  /**
   * Normalize namespace for topic construction.
   * If namespace is empty or root ("/"), returns node name with leading "/".
   * Otherwise, normalizes namespace to start with "/" and not end with "/".
   * @return Normalized namespace string
   */
  std::string normalize_namespace_for_topics() const;

  std::shared_ptr<force_torque_wrench_transformer::ParamListener> param_listener_;
  force_torque_wrench_transformer::Params params_;

  rclcpp::Subscription<geometry_msgs::msg::WrenchStamped>::SharedPtr wrench_subscriber_;
  std::unordered_map<std::string, rclcpp::Publisher<geometry_msgs::msg::WrenchStamped>::SharedPtr>
    transformed_wrench_publishers_;

  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;

  std::string input_topic_;
  std::string output_topic_suffix_;  // e.g., "" or "_filtered" based on input topic
  std::vector<std::string> target_frames_;
};

// Function to run wrench transformer (extracted from main for testability)
int run_wrench_transformer(int argc, char ** argv);

}  // namespace force_torque_sensor_broadcaster

#endif  // FORCE_TORQUE_SENSOR_BROADCASTER__WRENCH_TRANSFORMER_HPP_
